% 最远点采样
% inpoints为原始点个数，sa_num为需要采样的点个数
% outpoints为采样后点个数，尺度为sa_num * 5
function outpoints = fps(inpoints,sa_num)
    outpoints = zeros(sa_num,5);
    dist = inf*ones(length(inpoints),1);
    outpoints(1,1:5) = inpoints(1,1:5);
    dist(1) = 0;
    newpoint(:) = inpoints(1,1:3);
    disp('Farest Sampling...');
    for i = 2:sa_num

        %disp(['Sampling ',num2str(i),'-th point']);
        for j = 1:length(inpoints)
            if (dist(j)~=0)
                dt  = inpoints(j,1:3)-newpoint(:)';
                d = sqrt(dot(dt,dt));
                if d<dist(j)
                    dist(j) = d;
                end
            end
        end
        [~,index] = max(dist);
        outpoints(i,1:5) = inpoints(index,1:5);
        newpoint(:) = inpoints(index,1:3);
        dist(index) = 0;
    end
end

